#!/usr/bin/env python
import roslib; roslib.load_manifest('sandboxrfpc')
import rospy
from visualization_msgs.msg import Marker
import tf

tflistener = 0
NPoses = 0

def markerCallback(data):
	global NPoses
	global tflistener
	if len(data.points) == len(data.colors): #New axes have been added
		NPoses = len(data.points) / 6
	if tflistener == 0:
		tflistener = tf.TransformListener()
	transforms = []
	for i in range(0, NPoses):
		try:
			tfName = "/corrected_transform%u"%(i)
			(pos, rot) = tflistener.lookupTransform("/openni_camera", tfName, rospy.Time(0))
			k = 100.0/2.54
			posinches = (pos[0]*k, pos[1]*k, pos[2]*k)
			tfTime = tflistener.getLatestCommonTime("/openni_camera", tfName)
			transforms.append((i, posinches, rot, tfTime))
		except (tf.LookupException, tf.ConnectivityException):
			continue
	for transform in transforms:
		relTime = transform[3].to_nsec() - (transforms[0])[3].to_nsec()
		relTime = relTime / 1.0e9
		print "[Transform %i] %.3f: "%(transform[0], relTime),
		print "(%.2f, %.2f, %.2f)"%( (transform[1])[0], (transform[1])[1], (transform[1])[2] )

def listener():
	rospy.init_node('markerListener', anonymous=True)
	rospy.Subscriber("/rgbdslam/pose_graph_markers", Marker, markerCallback)
	rospy.spin()

if __name__ == '__main__':
	listener()
